#include "Body.h"
RCORDS Body::m_s_rtype = RCORDS::EULERQUATERNION;
unsigned int Body::NC = 7;
bool Body::SetRcords(IN RCORDS rtype)
{
	m_s_rtype = rtype;
	switch (rtype)
	{
	case RCORDS::EULERANGLE:
		NC = 6;
		break;
	case RCORDS::EULERQUATERNION:
		NC = 7;
		break;
	case RCORDS::CARDANANGLE:
		NC = 6;
		break;
	default:
		break;
	}
	return true;
}

Body::Body(IN double m) noexcept
{
	tM = m;
	id = -1;             //  id not assigned
}

Body::Body() noexcept
{
	tM = 0;
	id = -1;
}

bool Body::operator==(IN Body& other) const
{
	if (this->id == other.id)
		return true;
	else
		return false;
}

bool Body::operator<(IN Body& other) const
{
	if (this->id < other.id)
		return true;
	else
		return false;
}

bool Body::operator<=(IN Body& other) const
{
	return (*this) < other || (*this) == other;
}

bool Body::operator>(IN Body& other) const
{
	return !((*this) <= other);
}

bool Body::A(OUT Matrix3d& M) const
{
	if (m_s_rtype == RCORDS::EULERQUATERNION)
		AUX::EQtoA(pos + 3, M);
	else if (m_s_rtype == RCORDS::EULERANGLE)
		AUX::EAtoA(pos + 3, M);
	else if (m_s_rtype == RCORDS::CARDANANGLE)
		AUX::CAtoA(pos + 3, M);
	return true;
}

bool Body::SetID(IN int i)
{
	id = i;
	return true;
}

int Body::GetID() const
{
	return id;
}

double Body::TotalMass() const
{
	return tM;
}

Vector3d Body::Rho(const Vector3d& r) const
{
	Matrix3d T;
	A(T);
	return T * r;
}

Vector3d Body::AngularVel() const
{
	Vector3d w;
	if (m_s_rtype == RCORDS::EULERQUATERNION)
	{//Euler Quaternion
		Matrix<double, 3, 4> r;
		Map<Vector4d> lambda(vel + 3);
		AUX::R(pos + 3, r);
		w = 2 * r * lambda;
	}
	else if (m_s_rtype == RCORDS::CARDANANGLE)
	{//Cardan Angle
		MatR3CX Kr(3, 3);
		AUX::Kr(pos + 3, RCORDS::CARDANANGLE, Kr);
		Map<Vector3d> dq(vel + 3);
		w = Kr * dq;
	}
	else if (m_s_rtype == RCORDS::EULERANGLE)
	{//Euler Angle
		MatR3CX Kr(3, 3);
		AUX::Kr(pos + 3, RCORDS::EULERANGLE, Kr);
		Map<Vector3d> dq(vel + 3);
		w = Kr * dq;
	}
	return w;
}

Body::~Body()
{
}

unsigned int Body::nMode() const
{
	return 0;
}

bool Body::Write(IN Json::Value& body) const
{
	body["Id"] = Json::Value(id);
	return true;
}

RigidBody::RigidBody()
{
	Jc.setZero();
}

RigidBody::RigidBody(IN double m):Body(m)
{
	Jc.setZero();
}

RigidBody::RigidBody(IN double m, IN Matrix3d& I):Body(m)
{
	Jc = I;
}

unsigned int RigidBody::Type() const
{
	return Body::RIGID;
}

RigidBody::~RigidBody()
{
}

bool RigidBody::Calepsilon(OUT Vector3d& eps)
{
	Vector3d w = AngularVel();
	eps = w.cross(CalJc() * w);
	return true;
}

Matrix3d& RigidBody::CalJc()
{
	Matrix3d A;
	Body::A(A);
	M22 = A * Jc * A.transpose();
	return M22;
}

bool RigidBody::Write(OUT Json::Value& body) const
{
	Body::Write(body);
	body["Type"] = Json::Value("Rigid");
	body["Mass"] = Json::Value(tM);
	for (int i = 0; i < 3; ++i)
		for (int j = i; j < 3; ++j)
			body["Jc"].append(Jc(i, j));
	return true;
}

BaseBody::BaseBody()
{
	tM = 0;
	id = 0;    //BaseBody id is definitely zero
	pos = new double[Body::NC];
	vel = new double[Body::NC];
	acc = new double[Body::NC];
	for (unsigned int i = 0; i < Body::NC; ++i)
	{
		pos[i] = 0;
		vel[i] = 0;
		acc[i] = 0;
	}
	if (Body::m_s_rtype == RCORDS::EULERQUATERNION)
		pos[3] = 1;
}

BaseBody::BaseBody(IN VectorXd(*p)(double), IN VectorXd(*v)(double), IN VectorXd(*a)(double)):BaseBody()
{
	pFun = p;
	vFun = v;
	aFun = a;
}

BaseBody::~BaseBody()
{
	delete[] pos;
	delete[] vel;
	delete[] acc;
}

unsigned int BaseBody::Type() const
{
	return Body::BASE;
}

VectorXd BaseBody::Velocity(IN double t)
{
	VectorXd v(6);
	for (unsigned int i = 0; i < 3; ++i)
		v(i) = vel[i];
	v.tail<3>() = AngularVel();
	return v;
}

VectorXd BaseBody::Acceleration(IN double t)
{
	VectorXd a(6 + nMode());
	for (unsigned int i = 0; i < 3; ++i)
		a(i) = acc[i];
	if (Body::m_s_rtype == RCORDS::EULERQUATERNION)
	{
		Matrix<double, 3, 4> R;
		AUX::R(pos + 3, R);
		Map<Vector4d> ddlam(acc + 3);
		a.segment(3, 3) = 2 * R * ddlam;
		for (unsigned int i = 0; i < nMode(); ++i)
			a(6 + i) = acc[7 + i];
	}
	else if (Body::m_s_rtype == RCORDS::CARDANANGLE)
	{
		Map<Vector3d> ddq(acc + 3);
		MatR3CX Kr(3, 3);
		AUX::Kr(pos + 3, RCORDS::CARDANANGLE, Kr);
		a.segment<3>(3) = Kr * ddq;
		double s1 = sin(pos[3]), c1 = cos(pos[3]);
		double s2 = sin(pos[4]), c2 = cos(pos[4]);
		double s3 = sin(pos[5]), c3 = cos(pos[5]);
		a(3) += c2 * vel[4] * vel[5];
		a(4) += -s1 * vel[4] * vel[3] + s2 * s1 * vel[5] * vel[4] - c2 * c1 * vel[5] * vel[3];
		a(5) += c1 * vel[4] * vel[3] - s2 * c1 * vel[5] * vel[4] - c2 * s1 * vel[5] * vel[3];
	}
	else if (Body::m_s_rtype == RCORDS::EULERANGLE)
	{
		Map<Vector3d> ddq(acc + 3);
		MatR3CX Kr(3, 3);
		AUX::Kr(pos + 3, RCORDS::EULERANGLE, Kr);
		a.segment<3>(3) = Kr * ddq;
		double s1 = sin(pos[3]), c1 = cos(pos[3]);
		double s2 = sin(pos[4]), c2 = cos(pos[4]);
		double s3 = sin(pos[5]), c3 = cos(pos[5]);
		a(3) += -s3 * vel[4] * vel[5] + c1 * s2 * vel[5] * vel[3] + s1 * c2 * vel[5] * vel[4];
		a(4) += c3 * vel[4] * vel[5] - c2 * c1 * vel[5] * vel[4] + s2 * s1 * vel[5] * vel[3];
		a(5) -= s2 * vel[5] * vel[4];
	}
	return a;
}

bool BaseBody::Update(IN double t)
{
	if (pFun != nullptr)
	{
		VectorXd p = pFun(t);
		for (unsigned int i = 0; i < Body::NC; ++i)
			pos[i] = p(i);
	}
	if (vFun != nullptr)
	{
		VectorXd v = vFun(t);
		for (unsigned int i = 0; i < Body::NC; ++i)
			vel[i] = v(i);
	}
	if (aFun != nullptr)
	{
		VectorXd a = aFun(t);
		for (unsigned int i = 0; i < Body::NC; ++i)
			acc[i] = a(i);
	}
	return true;
}

bool BaseBody::Write(IN Json::Value& body) const
{
	Body::Write(body);
	body["Type"] = Json::Value("Base");
	return true;
}
